#include "Xunxian.h"

extern uint8_t x1,x2,x3,x4,x5,x6,x7,x8;
extern PID_Struct Motor_PID_left;
extern PID_Struct Motor_PID_Right;
extern uint8_t sensor_pattern;
int err = 0;

// 初始化 PID 控制器参数
void PID_IR_Init(PIDController *pid, float kp, float ki, float kd, float limit)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->integral = 0.0;
    pid->prevError = 0.0;
    pid->outputLimit = limit;
    pid->lostCounter = 0;
    pid->lastValidErr = 0.0;
}

// PID 控制器计算输出
float PID_IR_Compute(PIDController *pid, float setpoint, float currentValue)
{
    float error = setpoint - currentValue;
    pid->integral += error;

    // 限制积分项防止积分饱和
    if (pid->integral > 100.0) pid->integral = 100.0;
    if (pid->integral < -100.0) pid->integral = -100.0;

    float derivative = error - pid->prevError;
    pid->prevError = error;

    float output = (pid->Kp * error) + (pid->Ki * pid->integral) + (pid->Kd * derivative);

    // 限制输出范围
    if (output > pid->outputLimit) output = pid->outputLimit;
    if (output < -pid->outputLimit) output = -pid->outputLimit;

    return output;
}


// 循迹主逻辑函数：根据传感器输入判断偏差
void Xunxian(void)
{
    // 极左偏：仅右侧传感器检测到轨道（可能为左转或丢线）
    if(x1 == 0 && x2 == 0  && x3 == 0 && x4 == 0 && x5 == 0 && x6 == 1  && x7 == 1 && x8 == 1)
    {
        err = -15;
    }

    else if(x1 == 0 && x2 == 0 && x3 == 0 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -12;
    }

    // 极右偏：左侧传感器全感应
    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 0 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0)
    {
        err = 15;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 0 && x7 == 0 && x8 == 0)
    {
        err = 12;
    }

    // 左转
    else if(x1 == 0 && x2 == 0 && x3 == 0 && x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -18;
    }

    //右转
    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 0 && x6 == 0 && x7 == 0 && x8 == 0)
    {
        err = 18;
    }

    // 极左急转信号
    else if((x1 == 0 && x2 == 1 && x3 == 1) && (x6 == 0 || x7 == 0 || x8 == 0))
    {
        err = -45;
    }

    // 极右急转信号
    else if((x8 == 0 && x4 == 0 && x5 == 1) ||
            (x8 == 0 && x7 == 0 && x1 == 0 && x2 == 0 && x4 == 1 && x5 == 1) ||
            ((x8 == 0 && x7 == 1 && x6 == 1) && (x1 == 0 || x2 == 0 || x3 == 0)))
    {
        err = 45;
    }

    // 正常偏差判断（偏左）
    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -1;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 0 && x4 == 0 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -2;
    }

    else if(x1 == 1 && x2 == 0 && x3 == 0 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -4;
    }

    else if(x1 == 0 && x2 == 0 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -8;
    }

    else if(x1 == 0 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = -16;
    }

    // 正常偏差判断（偏右）
    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 0 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = 1;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 0 && x6 == 0 && x7 == 1 && x8 == 1)
    {
        err = 2;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 0 && x7 == 0 && x8 == 1)
    {
        err = 4;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 0 && x8 == 0)
    {
        err = 8;
    }

    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 1 && x5 == 1 && x6 == 1 && x7 == 1 && x8 == 0)
    {
        err = 16;
    }

    // 正中，直行状态
    else if(x1 == 1 && x2 == 1 && x3 == 1 && x4 == 0 && x5 == 0 && x6 == 1 && x7 == 1 && x8 == 1)
    {
        err = 0;
    }
}

